import sensor, image, time
import pyb
from pyb import UART

# 定义常量
UART_HEADER = 0xAA
UART_FOOTER = 0xBB
COMMAND_MAP = {
    0x01: 'LINE_TRACKING',
    0x02: 'COLOR_DETECTION',
    0x03: 'COARSE_AREA_DETECTION',
    0x04: 'BUFFER_AREA_DETECTION',
    0x05: 'QR_CODE_SCAN',
    0x00: 'STANDBY'
}

# 初始化UART
uart = UART(3, 115200)
uart.init(115200, bits=8, parity=None, stop=1)

def send_uart_packet(data_type, *payload):
    """发送UART数据包并打印调试信息"""
    print(f"发送数据包: header={UART_HEADER}, data_type={data_type}, payload={payload}, footer={UART_FOOTER}")
    uart.writechar(UART_HEADER)
    uart.writechar(data_type)
    for value in payload:
        uart.writechar(int(value))
    uart.writechar(UART_FOOTER)

def parse_uart_command():
    """
    简易指令解析函数，添加调试信息
    返回格式：{'valid':bool, 'command':str}
    """
    if uart.any():
        # 读取完整帧
        data = uart.read()
        print(f"接收到的数据: {data}")

        # 基础校验
        if len(data) >= 5 and data[0] == 0xAA and data[-1] == 0xBB:
            cmd_byte = data[1]
            command = COMMAND_MAP.get(cmd_byte, 'UNKNOWN')
            print(f"解析的命令: {command}")
            return {'valid': True, 'command': command}
        else:
            print("数据校验失败")
    return {'valid': False}

# #################### 主循环 ####################
clock = time.clock()
while True:
    clock.tick()

    # 模拟发送数据
    if clock.fps() % 50 == 0:  # 每50帧发送一次数据包
        send_uart_packet(0x01, 123, 456, 789)

    # 测试指令解析
    cmd_result = parse_uart_command()
    if cmd_result['valid']:
        print(f"解析到的命令: {cmd_result['command']}")

    time.sleep(100)  # 延迟100ms
